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[If ^JI 1 ] 

3 YKTtmm 1*1 \z & m\z 5-*. h ntz&n m % . ni^^^^'/fftte^a-^fti ^ 
tifrmmg: t rz\tmm^^<^~ kir-z-s-z. h tis 3 Kit ?ov^f mm ( 3 Kit ? * y 

[it $<m 2 ] 

tl £ 3 /Xtc ?o l Hh*fti(3 /^7U ?n^Ft^>F) % if SC^ti^ L . 3 
[ff^Jl 3 ] 

3 iX7C ? a v >f K t&M^^T^^T^ft-t^lf J^JI l Xti 2 fc!B«<7>»«ft0aP^a. 

im 1 1 



P = P 0 + £ u^v = P 0 + h £ udS , 0 < s < h, 0 < S = — < I 
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« = a 0 +a,5 + o 2 5 2 (3) 
(5 =b 0 +b l S+b 2 S 1 (4) 
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li * tl -P tl . 3 Kit ? n y >f h ft m X <50^<50 ft jg ~? 9 V >i . &V*<7>#JS!Hft^^-to 

hT*|iJofcfl^ST*ibto S l±&iX7C *J . zii*mmM^%tt&&, 

i , j , k It % tl ^ tl . x tt . ytfo.RVz$to?3&}<7>%.tiL'<9h>i-Z&Zo 
u It & P \z isrt § fl*ft<7)JtJj6^l«l*^-t ? h ^ T*#> 0 . 5^ ( 2 ) tc «k o T 4- h tl 

-5 c E k ^ i « ti 0 g; ^ h 'J ?x T 9 . * tl ^ tl x k fi * t> ^ <?> ft m. P <r> V 

j ibiiib 0 <7)ftl5a ^Hgt^fc LTi^. hu^-^ 3 - (yaw) % tr -v ^ (p i t c h) 

0Ktin = ^(2) It. i tfo?5ft<7> h >v& . * "f j ib * t> 0 tc « fin 0 L . 

<7) ic k 14 * ^ 0 tc /9 fi 0 f c t tc «fc o X . m HSL ^ * h^u^H^ti^c tt^LT 

a o . a j . a 2 > b q. bp b2tiSE$t° 
[M^JI4] 

— o<^3iX7C^ny A Y -b r > > h fciX<?>3iX7C^ny -f K -fe r > > N co m € B lc is 
Tx M^^fi^. Jfl^^fS] OkUMalz «k o T lift*) ^ilfeci" Siilc, Buie3^7n^ 
nvn*tr>>Fw7T(7)^7> - ? aj, aj> a 2 > b 0 > bp b j. h^fftti-£C 

^ff IS i: -t %m &m 3 k:IB«^8C«<lKlffll^^. 

im&m 5 ] 

fl^mmM: t tz lift^^^fCcJO — ^^T?-^-^. h tlS 3 /^7C ^OV^Ft7>>hSfflUT 
L . C <^> 3 /^7U ?nvn*tr>>M:J:o T X M <r> M WlS M B ~t & it fftfW 9P ^ S . 
[If ^JM 6 ] 

II c7>Mtft£$: M*'J W-t % fzdblz . 
=J > tf ^- — 5^ ^ , 



3 ftjtmm\H \z ttn\z-5-£.<bnrz}&m ft muffin v *j *n a * v a -n <?>^ 

[If^Jl 7 ] 

x m <?> mm* %t a.m w -tzrz it) iz . 

ZJ > tf .ZL — 57 ^ s 

3 }X7cJ^^i^ ic igtc ^> ti/c . J£*ft:frl«J<z> ^ -v ^ft is =k v 3 -ft tif 



[mm%] mmm 

[ 0 0 0 1 ] 

*^BHti. d#'; XfF^M. Siii, t«ti«£ ^^fitt ( n # -y Fftu 
[ 0 0 0 2 ] 

il^^^JUr-n LTA^^tiS. Lti'oT^ & HI 0% £ le ti . 4^ 

^> 7j >£ * ffl V> T % ffl ffl -f & £ . 
[ 0 0 0 3 ] 

[ 0 0 0 4 ] 

Li)X, 3 iftj** 7° ^ >f >fflMtt. »M^»«i**if fc*v>3ftil^f!C*«E^irSC t LT 
4^tltoTu5. Zc?y 3 iX^^T^ >f >*tWii, #iJ&^<b<?>^im«l i: ffl*<7>W 

[f£Bflfc*ftm L =fc 7 fctiSS] 
[ 0 0 0 5 ] 

2 /yXTCtefcv^T ii^f x. ^> fflWy^vS i: LT. ? n vM t* *t W i£ fc* flj # <b 

T * n V >f H 3 iyvTUlcM L . g i /&*U <7) ffl W Ic ffl o ft \t . ft*ft^^ $ ^ ffl » t L 

2 ft* ? n V >f K ffl^<7>£ff & <k *) . tcff o 0 . *ftj|lCJt* C T^fL£ i± 

9 -6 =k 7 & ftJflP % # ^ ic HIT § £ i: ® ti & . * . ftil^^'y LTu 

3 JXTClcM-f § z t li$({ift!J$n& ir<7)^ffk:4yv>T*MT*»S 3 i: *»JW^r$*i*. 3 ti 
*Tk:^ny>fl<ftJj**3iX7Ck:fl£^Lfc s t<?)i:L-Cl±x L i b<7) " 3D Discrete Clotho 
id Splines" (#!ff fF^CSK 3 #M) * ir **^n «b ti T 5 ^\ ^^^f y n y ttlt 3 
/yv7nlc^^i Lfct^tt^«btiTv^v^ ^^fl^T^SS^li. § fit * S lie tti T § 5 j&T 

[ 0 0 0 6 ] 

> 2 1X7C7 n >M H ft *l <J0 ^ ^ T S § 7c It ^1 § li <* ft 7c * 3 '<Xt€ ? n y >f K ffl 

<6*>£S$:iS*ig# L . 3^3 ^TC f a y >f h ft*ftk: =k o T ^JU * ffl i" § 

S o 

[ 0 0 0 7 ] 

[^Nf ffifcSfc 1 ] «8ijR»r • feffll8*f , " fE^-fbC AD/CAMyXfA" Aft 

. 1997 

[#!ff fF^i« 2 ] tfiBfffi , «ff#, ylffl7^#. UtOjff^, " tc £ § g iffl^ffl 

ffl m " + -y h ^r^li 8 ^ 6 ^ , p p 4 0 - 4 7 ) 

[i¥%$ff3C$k 3 ] Li Guiqing, Li Xianmin, Li Hu a "3D Discrete Clothoid Splin 
e s " . (CGI * 01, pp32 1-324) 
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[ 0 0 0 9 ] 

£U3-ft<n^n?nfrm&^^rz\tm&^^WL<7)^K^x°H-iLhTLZ> suit? p v 



L . 3 <?> 3 }X 7U ? a y >f K -b r > > 



ft ( 3 iX7C ^ n v >f H -fe r > > h ) % ffl u T 
[0010] 

m^<m 2 ^¥tmit , mu^tm & v *n *? * v a -fs ^ * nf*i*>*ftjj&ji * £ &ft*su* 

^ <^ - /X^ ? ^ i ^ *l £ 3 /X 7t n 7 ^ H ft m 
^mtmt . Z <r)WWL^<r> 3 )Xtu 9 a V >f H -b r > 

[0011] 



3 iX 7C * n V >f H 



[» 2 ] 



P = P 0 + £ucfc = P 0 + h^udS, 

u = E'*E*{l)=t 



0< s < h, 



0< S =-<l 
h 
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cos /? ~ sin /} 
sin /3 cos /} 
0 0 
a - a 0 +a^S + a 2 S 2 
j6 =6 u +^5+/» 2 ^ 2 
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li * ft *u 3 iX7C * n V >f H ft m _h <50^<jo 15 j£ ? h ;u . 7k U * <n f] Kfl M % X ~t = 

* h T*9J o fcffi* S-cifct. S liMiX7C<50MT*> 0 . 3 ti* ft*ftj|^$C t of 
i , j , k It % ft * ft . x Itt x y$l.&^z$i^freoi£'a^?h^T-£>£o 

5t(2)tcJcoT J fx.^tl 



. -eft^ft. k$i^ *> 0 <7)ft® (9 <7)@i^V 
S0#* 3 - (yaw) 0$^ &#fctr i/ * (pitch) 



£ <7) tc k 14 £ 9 \Z (9 fi It 0 ~t Z t \Z J: o T . }# -< * h^uA'f^JlSCctiLT 
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[0013] 

jc v 3 - n <r> z n^n ti*mmg: * a ft^^^fc^— iX^T--^-^. «b 3 /Xtu ^ u y 

h * ffl T ffl n L . C <7) 3 iX 7U f n V -i \t -b r > > h IC oTli^Illtil 



h <7> II g iC T x Pf # <7> jg 
£ «k 7 tc gO IB 3 )X7C ^ n V >f K -b r > 
. bp b 2 - h ^StfJ-t S 3 ^JNfiK t t« 



[00 14] 

If jf<Jl 6 <7>mni$. Xm<r>m$l}-& f &i$.Mffl'tzrzdblz. =i>tf~-?fc. 3Kitmmft 

\z a m iz 4- x. h n tz & m m * . m m ~n ft <r> v v * n & & u a - n ^ * n ? n * * ft m ^%rz 

i$mmM$z%z<ox.KjXx5-Z.<bnz 3 Kit ? * 7 ^ k -fc r > > h tfflv^TiSt^^t 

LTfl3^ 1± -5 <36 60 7° u 77 AT'i>|), 
[0015] 

ic ft miz 5- ^ <b *i tz & m m % . m m -h ft ^ v v * n & & u 3 - n <r> * n ? n t * ft m & * tz 

\t&m^k^L®t<r)~K^XH-JL <b*l£ 3 Kit 9 n 7 4 K -b 7 > > h 4fflv^TiPlt5f&t 
L T f& ftl 5 -t± § Tz db <ry -7' u 7 7 A X tt * <^ ^ n 7 7 Ale i^fH^I^EiLfcn^fa 

[f&Bfl<7>$j||] 
[0016] 

*%BHtc itilJ, 3 Kit ?n>MF-b7>>F <?> ±2E# to*ft jjfcJI * li ft m^k^&X £> 

^.hfi^cox. 3 Kit ?oV-fF-hri>h^l 01^^ L Tf# «b ti S v£*£7jlSK «k V 2 
0^^LT#^tl^ft^^ft*14%^f-tift,«4%^i(tcra LTjI^T'£>5 3 t to* HE 2 *l £ 
o s 1* tl If . ^ Ft 7>> h^)^T*lifti^^]fcJ; £/ft * to'iHroT* § 

o L/c^'oT, Vt <b to* T-'ttH & * n 7 >f K -b 7 > > N to*# <b ft . & IC %kM <r> & 

imn-%mM~t ztztb^m&tomm] 

[0017] 

JMT*2EHUlc out . 3 }X 7U ? n V KftH^SEithSfj^U 3 /^tu f a 7 >f H ft ^ tc Jc 
[0018] 

1. 3 #C7U ^ a V >f H ftHco^H tft ffl. 
(1) 3iX7E^n V ^ K<?)S*jt 

^nvMKftflMClothoid curve) . S!l =S ^ — ^ - <^ t^SSe (C 0 r n u * s s p i r a 1 ) t «t of 
fife*^ h fix -5 2 Kit&> ?nv^F ftl^ti . ¥ffift^ ( 2 iX7cffi*l) <7)-fiT^ »J . 13 

2 iz ^ $ n % x y mm ±.\z & i> x . Kx^xmz * 
m 3 ] 

P = P 0 + le»ds = P 0 +h£e J *dS, 0<s<h, 0<ZS=j<\ 
<p =c Q +c 1 s + c 2 s 2 =<t> Q +(j> v S+<l> u S 2 

z\z\X% 

P = x + Jy, j = v^T 

p 0 = ^"0 + Jy 0 

mwttommjjfa&^h-? h >i c^^^i^^^h^) ^^^^ ^ a 

iSifift ( x $6 7? |S] ) to* Jx If * t> *) iz lij ^> tl S o c <^ m & *< ? h >i \z Ii5«(/Jx4% 3 d s * toMt 
T ifft # -5 t ft ifft _h <7> ^ P to*^ *^ ^> tl £ . 

ft § l: )ft o T lb til § ^) ^ ^ b <^ I ^ s ^-<^^^ (4S*l)^»*iT^I 

*hkt4. s * h T-SU o fcffi* STibt, S M /X 7C <7> M T £> ») . Ztl^ft*|4% 



(l) 
(2) 

(3) 
(4) 
{5) 



? n V >f K fflJjft*>!ff ( 2 ) L «k 7 \z. mBUfaft <t> s * 7il4ffi&S 

^^ScSct) — iX^?^i?^til» c fclcl)!). c o . c } . ciifcli^j. <3^ v . ^ u li — #C it 

^ K mm * 5- £ S it 14 *n fi x & fc> o tz . %B!]tbli|]fcTCii§i|ufc 0 
[$t 4 ] 



P = P 0 + £u</.s = P 0 +h£udS, 0<s<h, 0<S = j<1 (6) 

cos/3 -skip OT cosce 0 since! fl] f cos /? cosce 
sin/5 cos/3 0 0 1 0 < ol = < sin /3 cosce > (7) 
0 0 1 -since 0 cosa 0J -since 

a =a a +a l S + a 2 S 2 (8) 
P =b 0 +b i S+b 2 S 2 (9) 
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u li^CP iciJtt* ft*ft<7)Jt*6^l«I*^-t^fi^ h ^T*> 0 . it (7)lc «k oT^ibtl 
So it ( 7 ) lc o T , E k P #c V E ) « 14 0 $5 ^ h '1 UT^I, 13 3 \z ^ £ tl -5 =k 7 tc. 
* ft f*u k $6 ( z $1 ) * -b 9 <?> ft Jg 0 tf> 0iS V j Mi ( y $1 ) * t> *) <nft a 0 $k ^ 
^fcLTi^. b0#£ 3 - (yaw) 0$S. tttf'yf (pitcli)§ii:U7o it(7)l4. i Hb 
(xltt)^fS]<50^te^^h^^s St j ti (yi) * 0 lc « It 0 L . L jb- S <?> *> iz k 
f* ( z W ) £ ^ *J tc (9 fi it 0 i" z i: ic «fc o T . il^^ 7 h^u^ibtl^C t^^Ltv^ 



"t & b . 2 ^7U<?> if 



3 ^7U^: 



- ft (9 fc> <b m 5 Z t T* § £ o tT v * ft a 0 tz t . xyfffl?to/:2iX7C^ovn* 
ffi*ftfc*#<btlx 3 -ft (9 /it. y zfffi-?tv^fc25X7Ci r ny >f Kft»**#"btl4. 
If ^^0^0-^ f h ^ u lcj^/Jx^$ d s ^^li-Tli^-tS 3 YKIt ?nvn* 
ti S = 

^ ( 8 ) Jk VjX ( 9 ) ic ^ i- «k 7 iz. ^m^^^S^ 2 ^ it T x ^ ti S . z^chlcioT 
m^^0<^^fb^ S i tc jg v * ** h s =5rfc^o . ^^^fbtcii^'tt^Jf fc** c fc o*dT|6 
tc * & o 

KI^itlCokoT^Lfcrh < . 3 )X 7U ^ n V -f K ft H li r t% B 1i 0 ^ X v * ft 7k U 3 

-ft**. ^ n-etittJjft^^sc^-iX^T?^*? $ ns ft j hfemztLZ* 

[»5] 



a 0 , a,, a,, fr 0 , Z?,, ft 



(11) 



>-57[ Cc fc 0 Tft/^^tlSo a 0 ^v^Lb 2 ^6o<^^i(t4ft)5<^i|ifl^|f*> 
, j7 n V >f K -fe r > > h comVt^^t) L T 5 . Z lc *t L h 14 * $ * * if . 
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( 2 ) mmm 

^ ( 7 ) izis^x . **fi 
b „ #v co ift ^ +i (moving 
[& 6 ] 



ft ? V >i i <?>n *) \z 
r a in e ) E ^ W -S „ 



: * ffi ^ ^ [ 1 



j , k]^ftAt§ 



£ = [ u v w] = E kp E ia [i j k] = E* E Ja [/] = E# E Ja 
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sin /3 sin a 






— sin a 
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cosa 






cos /3 cos a ' 




r — sin /3 




cos/3 since 


u = 


sin /? cosa 


h v==- 


cos j3 


s w 




sin /? sin a: > 




[ -sin a 
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(12) 



(13) 



v R v w a ft jj$ <t> f§ j$ tc S it 4 ffl ic # * ti £ i£ ^ ? h ^ x* *j . 5 vmc 
on ttfftj&P b b ^izM < 7 V - a (Jffi£&. Igtffi) »K 3 ti^l^fi tin. 

[&7] 



E 0 =E kha E Jau 



(14) 

(15) 



i: ft S. 

[ 0 0 2 0 ] 

( 3 ) n - •) > r 

^:tl:J:oT, 3 oB<^0i T^-^ (roll) J ^ft 

i^W^Jf x. ft o 3 )^7U^ n V tcS|ip5tl-5ift^fll^lij^W-t-5 
M^lcii L fciSItt . #+^<t> i b U- @ i lc -5 3 b fc*X § § fc* 

T #+ & CO ff^ M x. § t> rt X tt ft 

n-^ 0f£^#JSi-£ h § . ift^lftiT^ t ft So 
[St 8] 



F St 
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<T>WtV\\z It 
■Z <t Id o 



£ = £'"' / = E ip E ja E ir 
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a — >v 
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y = c 0 +c, 1 S' + e 2l S 2 
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(4)3 /^7U ? a y >f K ftJj6^»M^»tt© 
( a ) 3 ^tc ?Dvn* ffi*&<7>v£*l 



£ & Z b ib'^D 



[11 0] 



n : 
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(18) 



(19) 



-<x'(S)cosp(S)sina(S)--P'(S)smP(S)cosa(Sy 
u '(S) = « -a ' (5) sin p (S) sin a (5) - p ' (5) cos p (5) cosce(S) - 

-a'(5 , )cosa(5') 

^ J 

Hu'(5)|| = V«'W 2 + /3 '(S) 2 cos 2 a(5) 

-fSr^*,. 3 ^tu f n V >f F ft *fc <7> Jj* ~« * h ^ it . S % ffi X TIQ^^T^ $ ti 5 
[111] 



n(S) = 



u'QS) 



Ja'tff+P'iS) 2 cos 2 a(S) 



~a'(S)cos p{S)sma(S)~ p , (S)sm p(S)cosa (5) 
a' (5) sin j8 (5) sin a (S) + 0 '(5") cos /? (S) cosa(S) 
-ct'(5 , )cosa(5') 



(20) 



[ 0 0 2 2 ] 

(b)0i&*fflv>fc3iX7C*nvM H ft&£<7>v£*£ 

( 7 ) <7>Jt*ftu<7>^Si:|^l«k:^*ftnk:ov^T ! t%^.TAS. #J SB Jt *ft?j ft ( 
■ 0,0) ICMLT. liaS^^ltSiy^fflv^ ( 0, cos v - - s in v ) T^fctttS. 
d ft* Jf*8 i: TO C J: 7 ic 0 IS £ -t± £ t . v£*S n T 10 <^> J: 7 ic ^ $ ft £ . 
[112] 



cos a (5) 0 sin a (S) 

0 1 0 
-sincc(5) 0 cosa(5) 



cos P(S) -sin P(S) 0 
n(S)= sinp(S) cos/3(5) 0 
0 0 1 

- sin v cos p (S) sin a(S) - cos v sin p (5) 
- <j - sin vsin p (S) sin a(S) + cos v cos p (S) 
— sin v cos a (S) 



0 

cos v 
-sin v 



(21) 



(2 0) (21) tf>5*% -5 t . siny, cosvliTIBlcjhtJ^LTv^SZi:**^*^ 

[113] 



sin v = 



a'CS) 



cos V = 



yla'iSf + P'iS)* cos 2 a(S) 

P'(S)cosa(S) 
ja'(S) 2 +P'(S) 2 cos 2 a(S) 
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[ 0 0 2 3 ] 

( c ) 3 ^tc ^nvn*iilc felt £ If^^T^viMilic 



[114] 



fr\ ii^cT* 3b tl It J: V* c t fc* £> £ <> 
[ 0 0 2 4 ] 

( d ) 3 7u ? o v >r h Eft m <r> ft m 

[115] 



K(5)= A — <24) 

(19)5£J;!>, 

Ja' 2 +B a cos 2 ct 

kt(5')=^ ^ (25) 

h 

[ 0 0 2 5 ] 

(5)3 /tvtu ? n vM F ft H ^ iff & 
( a ) ft^^a^ctt 

— o ? a V -f H -fe r > > h ( fl] — <7> ^ 7 > — 9 X*$kt>$ *LZ> ? n V >f H ) v» T li 
» **>8r*&;frl«Jtf> f -y *ft7kU3 -ft fr J til j etL&mM^%tS<7> 2 iXs^T-*?- £ <b *1£<7>T 

S lc n L T ilMT* *> S C h fc* ff SE 2 tl £ . a & £ tl «\ — o<7>?nvMK-br>>htf> 
+ T*av£^7^f«I^^ft*^3i^cT**>^ o Lti'oT, ^Tl4H<7>Jl^ft^jb*t#<b tl£ 

ft^ ^ii^tc ft & =k 7 tc ^ 7 > - ? *3BiRt" 5 3 t tc Jc o T . m <b ft ^ t o ft iTco ft&t 

[ 0 0 2 6 ] 

( b ) m m & 

ft Jjft^ - ft * — o Co ft m. ( ^ 0 > ft 7k v a - ft ) T*Jg£ c h 5*5* 

* ft * ft tc t> -t± tz 3 <X tu ft m £ ft « tc ft § C a«T'^ ft ffl j£ tc m * 3 fc jb* 

[ 0 0 2 7 ] 

(c) »f6T ft 18 l^te 

fi.*6 • P3 # • fa C ft ft £ com® ft if$ ti x ? n vM K ^ 7 > - ? < o 0 ic L , 

*>* vm± x < o fc»*> m ^ > - 9 mizift feco mm. m %> ztiz&^Tftzzt 

S = 3 tl «b <7> ft *| a ^ n V -f K ft m <r> — ft ►) . ?nvn^7t-7n*)fiuT 

ilT'S §0 Lfc^oT, ta£*^N C <D J: 7 tc. • P3 # • S ffeft^^lc J: o TieiEi" 

5 7 ^ - -7 v V % ^ ^. T 3X 0 7 l± ft < . 7 t-v-y h tiuTlflLt , ftlj 
I L t 1 T § 5 . 

* tz x a Sfctt^uf tl*^f IC 0 tl< 3 tlCioT, 2 #C7C ^ a V -i K * ft * 3 

t ib*T* # ^. <r> T' . 3 tl * T 2 iX 7C ? a V -f K \Z o T i" T* tc # <b tl T § H * v£ ffl S 

i-ft^*>. fiilc^a ib tiT v^s 2 ^ a v 4 k * #a6T . P3?R^fi.^ft £coM%]<7>mm 

tx ^I^lC^Sf^C J:T*I1T'§§. 3 c7> «k 7 ftfiSU^ft^lC o V^T [1] — cOff^ 

^ 3 Kit ? a V >f H ft*ft^*fflv^S Z i: **T'§ tfS^HH * ^-MfbT § £ 0 

[ 0 0 2 8 ] 



(d) Mil L^H2 

3b £ VMi ^ g& Fft ft ft frfr is* *) \Z < \^ Z t fc*#v^ . 3 JX 7U n y >f NMC X It . v * 

?o'MH ft IS t iTil L fcj&Jffilc«UI • ft • ft*V<7Mfil±fi3E£u * o 

Tfc*K 86*wiBft «k 7 ic . £> <b fcftT If Si" t ^ b , ft *ft > ^ ^ 

> - ? S fcttlfc L X . ( 7 ) , (20) JkU (26) sUc^i" «k 7 ic . ft ft*** 

:w;tl:J;oT, ^ tc 1+ S iff m * M ^ L . > =e - 4- if <o W 35 ^ fp #9 -t £ c t frx § 
. t >i 9 -i uxcomWM M^^imiz-f s o 

NC^Dlicfev^T, IMih»^M^ft*#^tiMft ^ifT£> 9 . x-y ^ >f i: 
T li 3 tl * ^ -5 <7) [C ffl 15 J ft tf S ^ ^ ~? £ Jb* . ? n V >f H T ti ^ A£ fC -te r > > h r i: IC S 
/Jx ft $ ^ g <^ ft fri x 3b £ Tz ib . ij v 9 - %<r> Mfe ft £ lc X 1r fll £> £ . 
[ 0 0 2 9 ] 

(e )jf ifrffeOffll^^ 9 ^2 

-t ^ n m^n^x h- x. h ttx \^ . zcntzdb, &2 s -zmm t com®tt Lxfetbz z tiz 

=koT. flnjftifft i?<oMifj£J 'tt^ffSUc Jf -5 3 t ft'Ti . # a ft if ic ffl u <?> tiT § 

il § s liH&^^^T^T^^MlciyltSffti: LT 4- x. «b tlx • ftnMS & tf*S^J ft lc 

ftv^ 0 ft*<7>tfS^^^ft^^. atdnf <?>3i'llN^nM^ t §1 Icj|<^ «b ti. Mlftfi* 

tc fa. C tz mW * ft 7 c 
[ 0 0 3 0 ] 

(6) ft , 1>! ^^>-?<?v|4H 

^«lc «k tit* 3 "Ait 9 a V -i h* ft ^* ^ > - 9 ^ftHlc^iSi-^WlililT^ 4? 0 
X 3b Z> o &^=> >-?^4i-5 3i:l:ioT0 3^)J;7lc3^7U^n V -f K ftJjfi^^J^f 
Z z t *'t§ § . it z <7>jf ^<^ft!^<^ft^^^ic S fc ft* « <^M#ii0 4 <t> «fc 7 tc ft o 
Tfc . 3 /Xtu ?d-^H ttJjfitiftifft^^SC t ft* i: § 3 JX tc ft ^ % ft It 5 3 

im i ] 







k: =( co^xr ) 




K CC 5 




^3 -( CO«A7 ) 


K » { Ctf/Ztff ) 









3iX7c^n v -i h ft mx it . ft ifi <r> 'i => > - 9 S tc W "t £ ft * <?> ^ ft^ TflJ L ^ i" < . 
m^ft^fttc$iJiJi-§ c h Jil T ck 7 icSHT'S 5 o 

• a frfe®. X co 2 iX Jl ** ft It tl It . ft * It - ^ fft T« 3b Z o 

• a (9 2 /X<7)JM^*tia\ ft*ii^^^^c?>l?gillcit#l|T'*^o 

• (9 a <r> 2 YK <r> 1% to* % ft *L \t . ft*tt — SflT'ifc^o 

• (9 a <7> 2 /X<^JM^*tia\ ft*ti^^^^<^S^|lctb^JT*^o 

[ 0 0 3 1 ] 

2 . 3 JX 7t ? a V >f H ft if6 tc «k £ ffi n vi 



( i ) m<bfr%mm<nm^83ikft 

tc Jc £ X S Mill ffl * ± * B & t LT, 3 iX tu 9 o V >f H ft *g ( 3 iX tu * o V >f h'tr 

2 3 /Xtu ?pV^H ft*6* , ^<^SS^T^'b fr\zm$c$ fix ^ £ z t it . Jftj&feg. 

«k ^ft^^ii^icm^^ n-eo k!: t**>s t ns. ±ifc<?>£ifcj$*fflv*T 
mi 6] 

Px,Q.) = Px M {0) 

Pz / (l) = Pz, +I (0) 

a,(l) = o: f+1 (0) (26) 

J3,(l) = j5 f+1 (0) 

tanv f (l) = tanv, +l (0) 

fc f (l) = sr m (0) 

3 tin. mm&xmm~< 9 h >i t^m^ 9 h 'i . \&mt a . p mtmx&zrzdtxn-tftik 

^W*l£z.Z> Z tijXB 

mi 7] 

P Xi (l) = Px M (0) 
Pz i (l)=Pz M (0) 

cos[a,<I)-ce w (0)] = l (27) 

cos[£,(l)-/3 f+l (0)]=:l 

tenv l (l) = tanv w (0) 

K t (.l) = K M (P) 

zz-e£t>ic, 

cos[a f (l)-a ) . +i (0)] = l 

tft % If. ic v ^5 fc 

tan v,(l) = tanv f+1 (0) 

li, TIE *.e>n3. 

tan v,(l) = tan v 1+1 (0) 

«',Q) _ a' M (0) 
/?', (l)cosa.(l) j8', +I (0)cosa, +! (0) 
•••«', (DiS' I . +1 (0) = a',. +1 (0)jB*, (1) 

Tm<7>& ft % H li B © 5 3 i: fc* tt5 * * 3 t fc* t> -5 o 



[818] 



P Xi (}) = Px M (0) 
Pz i (l) = Pz M (Q) 

cos[a f (l)-a f+l (0)] = l (28) 
cos[/5 f (l)-0. +1 (O)] = l 
a,(l)J5,. +1 (0)=a w (0)jS 1 (l) 
k,<X) = k m (0) 

[ 0 0 3 2 ] 

( 2 ) mfc®%3\-M^m 

[ 0 0 3 3 ] 

7 tc L T . A * t C M h fr\Z fiifro-t § 3iX7t^n y ^ K ft*H^^£ fc*T* § -5 . 3 

<?> n- s # mb ic =k *ua\ ftjjft^ ^ > - ? <7>gft ») . c ti-^mm 1 0 c<^^^ 

Si" £ c i: ttT? # >5rv>o 
[ 0 0 3 4 ] 

(b )i^it>^^^ firz^m^^m^mm^t *«j:^ic, 3 ^tu ?n«MH ttn^mMi- 

ttl (^o>MHtr>>h) 3<7>Jf^icti. (28) ^*Sffc-t ck 7 tc ft3£^° 

v > - ? tm^JWtt (a ) «k »J «*tT?*> 9. <* »> jM LiR^s+ff i: * * . z^ffS 

_hie (b ) tf>5##lc o v^T . tf B^ v£ * f¥*ffl tc "t * . J# < ^ § If S ^ H It . #^£7 
tc^iUL^ 
jfefti ^ => > - ? : ft ft ^ ^ > — 7 

i'JIS^ Jf^tc li, Mtcfflv^ 3^tu^ n V >f K ft*fctf>8c* ^SO ic «fc »)it^L 

±12^** *J 51 LtR^I+S^^/^tciR^^ i±£ fzdblz It. H-g±tf>I3:**'&^T?S>S. if 
5:MitMT-i>5. 36 tc . x. «b fifzmm^^ te>ffiM&f¥-&mrz~t . =k »J 

$Slcf»t#§ff J:LTf^T*f>5. ^ijxti\ ft*l^^^^|l|I^)X^ «k 7 *H 
o^STEP tc^ttT. HSiXUfft*. IlSTEPh L T MIS h gJjft^j f«I Jb*-gCi" 
S =k 7 tcffiP^ Lfc^T?. S2 STEP i: L T I«I * -BCi" -5 <k 7 k:ffiM*ffv^ 13 

ste p TtMt-st-rs =k 7 icffip^-t^o zio^vi^rtticoffit^^m 5 icie-to 

[ 0 0 3 5 ] 



c 3 ) m&®%atnm 

ZZX'lt. g tfe ft^HI Wfcte©. |fH?Jl«K Sc^TjftU MlC o v^Tiiifro*0T 3 Kit 

3D Discrete Clothoid Sp 1 i n e s <r> Q <r> #.^ff2T* £> £ ffl 5t t^?U n lc TR^^ 2 o 

Discrete Clothoid Sp 1 i n e s ic o T Mm ~t £ . * i* . fct W W £ #1 ^TR^ 
i: -t Z m P * fF *) . P <?> &17C;& W ic la] D » r ® o*o $f fz *T7C J&fcJf A L . P c Q i: 4 
£ J: 7 * #ft ff^ Q * fF 3 o CCTP^Mii'nifcSfctSJ:, # V =f > Q li g§ C T £ 
i^Trnl, ioT^SS^Tr ( n - 1 ) + 1 flM <7> TR^% Jf o C i: 1C * & . Wik*f 7* 

* ? 7' h *46^*^<7>a Lit t LT . §TI*^ a i T'if: t t c i" S „ §TI^ 

rie^j^ (29) %mtz l . ^ (30) <?> us # =r > q * 3 

D Discrete Clothoid S p 1 i n e s t H 7 „ 
[SI 9] 

|qwqi|=|qh.iqi|. (q s «p) (29) 

£ |A 2 * ir+1 1| 2 , i = {0.„n- 1 .}, A 2 ^ = * w - 2*, + (30) 

3 }X 7C a v -f K ffl Pi6 ^ fz db m M ft It x r = 3 <7) 3 D Discrete Clothoid S p 1 i n e s <D * 
') =f> Q^fF 9 . * d <)^^lT-i*St§. v > - 9 aO. bO[cov->Tit (31)"? i* 

* S ? N n t J: 9 § „ 

[t20] 




(31) 



3<?)Jtifi^l^^^h^ t it * •) =f > Q % 5)< t § lc f T lc fffi £n i: 4 0 T i> 9 . c t 
tit(7) t \z =fc *j § ft iji <?> a 0 - b 0 i)*$Z * 5 . c <^ § # 'J =f > P <r> TB^T- # 'J ^ > Q 
tc o T t a 0 , b 0 * :d<#> T < . 

::T\ 3D Discrete Clothoid Sp 1 i n e s co %_m. <r> ^ (2 9) t 3}X7U 9 u V -i K ffiiji<^ a ^ 
it (8) t *>^i±T -5 Tie<7>it (32) ^o. 

[if 2 1 ] 



a0 3i +~al 3i +1 ; j a2 3j = a (),._, 




a0 3i +|alj, + ^ | a2 3i =a0 3(i+iw 



(32) 



a2<7>#]»Hi t -t*. Is] tile >^=> > - 9 bl, b2c7>nSBfft 2 t v*^T-§ ^> 0 



[St 2 2 ] 



h 

_ V(al 3i +2a2 3i ) I +(bl 3i + 2b2 3i ) 2 cos 2 (a0 3i + al 3i +-a2 3/ ) 
n 3j - - td 4J 

<7) ^ ^ > - 57 <7> f] $ ft £ 2^ i - h > • 7 7'V>ftlC ioTtft^o 3 tl IC J: o T f# «b tl tz 
> - 57 3 }yv7U * n V -f F ftfMX/& L . j^mm* 3 ?o'MH ttiT'iKt 

* . ffl n m * m 6 & u m 7 \z x •+ . 

[ 0 0 3 6 ] 

3. 3 KX t n V >f F fflW*fflv^fc»«ftiJ«P^^ 

& v . mm^tit* m h &\z -t % z t t^mt^ z t x$> % . 

[ 0 0 3 7 ] 

(1) 3 /fXTu 57 a v >f f nm^m^tzmmrm^^ 

o 

[ 0 0 3 8 ] 

(a )XMM©jfLi»<7>^tf (08. SI) 

hl^IMA'KtS, f^IM^fti^ (I1,>S, tool center point) 

tn-^. IMj&*>&HI±. Mlf? (x. y. z) T-H2*u IM^fSfl, 0J£ttx 
. y. z 0$S/% STi^ tl£ 0 ir-<7> J: 7 £M£t&©)§ T ^ . X M ^ fl» 3* 

tfltiaitfltuc £ S c i: # < . Ii»lci<)'oTv^. 3Iift^JSP<?>m 1 SFStt. C60|fi»<7> 
ff^t^. 3 }X7E ?dv^H ftHlc^ff-tS 3 tlz3b&, 
[ 0 0 3 9 ] 

( b ) MWl ft m<v ^ X It (138. S 2 ) 

%ti$.mmfrh<D^miz jc 0 . 3 )Xtu * n v ^ f mmmmizm-? x . mmx^mmn^^ 

< xm&omm • Me^^s-t^ z tx&z, wi& ±.-&xm * i xmmomwc 
tLxm<fri$. xm&comfs. - 3 txfetb^iiz* xm&comm. • 

-ffltftizitmmizttLxmfez iizm^fr^^fr. m^itmrn^x^-t ^>m^. x^kwi 

<7>Jf^lcttBt LT iil S fc*i*J£<? ti£* 

* n mff^ «j tc u t it . m it ii u mm \z um 2 nx & <r> \^ ft m& u m -t z> * 

^<7)-o-o<^ft^lCall^ft^^ iaT <S6 . il^il ^ fsfc-? Z . 77 ^ t T > ^ ^ t 
ti. H^T'5ulcI^t^ x, y. z ^ 3 t^^m^'X ^ < hfl^> 3 )XXmm^X 3h . X 

[ 0 0 4 0 ] 

(C ) E^7>S0 (08, S3) JkUJl'l ry7>lH,%lC J: 5 X M ^ fl g • ^i|<7>tf-Jt ( 13 



8 . S 4 ) 

t b . ^ n^n^mmiztt^ zxm&^^&Tt^db % z t frx° b % * (c)^tt- 

Sli. mfatiSlz itliiT^ J: 7 left ftibiiZ* M1±£jz fcv^T . ft a 'If 35 ^ It $g . 
^S^ft*^;^. CftlC J: 9 ^ift^<7>tt*|jl^ v > - 7 ^tfST*§ So C <7)^|^^[iiiM^ 

^ > - 9 iz j: . wm&^&izisi-t z&w.mm*<>m m. t^t^^it§ c t ft* 

fc^&W. t^Mb^M-Z ftS, t Ufl, 3 ^TuTli ( x . y. z. A. m . y. 0 
) i: 55: -5„ fcCL ( A * m . y . 0 ) U^iE^f L/c^T ( A * m . y 

) V fffi 0 $5 ill ^ . 9 ti 0 ^ ft ^r^i" . 

isfc. SScfi^Jfift^^W^ic ck 0 . 3 YKJt * n v >f h fflWft*ftfcv&o *fc ft ^ }g 
£ ^tv££l+ * 7 -b -y h L * 7 -b -y h T . 3 ti* ^7 >y ? - m 7. (IS4"l><7>fiS* 

) C<7>ffW^. ft ft*J|< * o T V* S <7)T . #|T'J)5. 

[ 0 0 4 1 ] 

(d (0 8. S 5 ) 

Sli — J&lcitff&fltfl? ( inverse kinematics) t of it ft X u S . $ !l 6 Hb <?> n # v V ft* £> 

stts^ wffj**6 m^ngitu Bfc^wtiK ^c<5owffj. ^w<7) re kj # ft* m 

S o 

ttitstf ^T<^tfSft*^^h &So i#«i*jt»m. a # -y h ^<^mii<7>Mr t tcufir 

[ 0 0 4 2 ] 

(e Jttffi^^k: «k S &iifct- ?^ftc7>ffS (08. S 6 ) 

Bf7>9J $ nt^IM^to §]£fa«8P*#«>* d n*^-«it- 57 (jftSft-r ^ - 31- y 

(ft mm) *m^x . ^;ui^^i$ft^fcf-nu$#)^ 

±15 x ( a ) ( b ) Itmm^Stc^imx^ *) . -Jg/*ltfr * t>tiS. (c)-(e) 

±IQ<7)^T<^g+S^ . ®C M $IJ P ^ rB <7> * T tf 7 C ^dT^T-^ 0 . <fcSvMi. ( a ) Ik 

u ( b ) % su<t> 31 > - - 9 ( if nm) \z k *) i\-MJkvm%. ltjj§, * <r> \&m 7 > - 

y * if^^fftiOP^glcM ») iA^. ( c ) - ( e ) <7>tm*&f£ffe!Jffl]!§E!l*3-?fT 7 c fc t 
[ 0 0 4 3 ] 

( 2 ) N C^Ih C N C ^a 

WTx ^^^^ffiftiJP^rH ( N C ^^ffli"SJt^t. 7° a r 9 v^^flj^ff o tz 

zi is ^ ^ - 9 tN cmutfr-faitz nrzc n cmu*&m^ zM&iz-D^xmw't &o 

[ 0 0 4 4 ] 

(a)^3A<^NC^g^fflv^Sit^- 

^*<^ii^<^ N C tlMT'ii . y' n r 7 5 > r^ftoTNCr-^^ff^t^yn 777 
i: x C <7) N C ^- 57 ^£ ffl v^T tiM^rM^i^ft^-t N C^gt<J0 — o^^gtA- H "7 x T 7 ft* 
5>I^tlTv^„ ^ tl tc W L T x C N C iWiT l± . 7' a 7 => 5 > • r ^ ft 7 =i > tf ^ 



- 9 IZN CmW.\zftm$ tlX . - # ft 2 tl 7z t <7> i: & o T £ 0 

£ ~? . BU % <50 . ^aZI^ON C^Itffll^falCOl^T, 3 )X 7U * a 7 >f K IC J: § » fl fell 

8P^^*ffilg-r S. C^Jt ^-. ? n y ^ k t-- ? tti L C li ? p V K a- 7 > - ? $ 
ffloit^tU G =i - H <r> ^ ic ? n y -f K <r> ~7 * - v -y h £ Sit -5 . Z titt $J z. It . 

G *fc *fc *fc 

AO, Al, A2, BO, Bl. B2. H 

G***ttG3-H^)|ft*t. AO — Hti 3 /Xtu ? a V -f F -fe r > > h <r> 7 
-o<?>'*<7 > - 7 t^t. Z^3-K*Htft5ilc, ISllP()^fiiaic*Tv^, NC 

c^^fp^ riw»J tui. ik»*n cnuffl-z?f im&iz^- vco^mit-zm <• 

@ ft jb*<b ?#>S ft*. * #>IC N C IgT'fl . *> § ft <r> 'MM * <fc^ ^ -5 . Gn-FT^ 

m t & * c 

[ 0 0 4 5 ] 

(b ) C N C ^ 

7' a r ^ v <7> * o 3 > - - ? fcNCt§Hi:<7>--frft:£ttfcCNCi§g<7>fl§£- 

? a V A Y \Z m~t £ If ^<^g^<^ — H T?tf tl £ 
SSti^'ivi. £ tz . t' - ? <r> m $z M <r> * f - K «t W i* § ti o o § . 
— I£ ic . c^7*n vicii, * fi f ti </> ^ [c iS L ? a v K <r> >i=j > - 7 % v£ S 

* 3® * fl £ . 3 tl * r il » J ff IC tt s fjiif, U < O fc* <7> HlfC & 

) *b # * tl § C t > o 
[ 0 0 4 6 ] 

c 3 ) 3 /^tc ? o v ^ h* ffim*fflv^»faftijap^^<7)j|f m 

[ 0 0 4 7 ] 

Jlll^ifti'^ > - 9 IC J: >)il$nTv^X^7 > ft * if <7) ftfe<50 ft *| T" . ^Sft 
3 tl m "t 5 ic x 0 9 tc ^ $ tl -5 «fc 7 tc . 7x 7° 7 -f > ft H R ( t ) T ^ § tl § #i» _h 

^ft*l^ *mc if &~t z> t <?>Tii^v^-e. ^3A^fc<Jo^fb» a tait^icii^fticfc 

li?S4v\ R 0 +AS = R(t 0 +At)<7)jt^^v^TAt^?t<a6^ti-tia\ S^^^ff 
tilts c fc *v^7>t?, -^BfreraFs^lc zc^ffS^^ »j 5fl:tttili«: h z t 

[ 0 0 4 8 ] 

(b ) 3 YKtz ? n V >f H ft*STli. ft iji^ lc S tt^c?> ^ fb<^ tt^J . fifWWtc-^T 

z t frM'&z fi. znizttm-tz&mmwmmit. mmmm^m^^^> . ti^mizm 

m<r>'J?*\.*MWfemt z t frWWZ tis. >f *r-?i±. ft^^fb 

[ 0 0 4 9 ] 

(c ) 3 iX7C^ n V >f H ft*ftl±x *tf>Jf#5fc&Jf£-i: LT, P3 # . ISi£tt*6«:^*# 

A,-? 4? ►) . MSO^ft^^^ffl^aAt? :U<, ^ ftllic ttt * 8c«fMfflH»S5*jKiBk: 

[ 0 0 5 0 ] 



(d) 3 YKit 9 n v \t mmitmmMi<^ t *) ftlz J: h . x, y, 

z Ah T* ft *§ 3 ti 3 <?> N C m H T li . 150 x. -7 - ? % ft a6 ic ffi it T fin X ~f h t £ iz 
. n - ? <n SX v ft t+ ft \z J: o T it . Ift^I^WILtt^of: 9 . ftn X L ic < ■? fz*) 
-f Z> Z k t)*3b& <, 3 iX7C ^ o V >f H ftJjSTli. ii^ilic J; o T4i bilS^T, f4a6 

fin X § -5 c 
[j^3IX<7>fiJffl5rik1£] 
[ 0 0 5 1 ] 

*%bh<t> 3 /Xtu ?p 1 MF ft^fUc intt, X^Ma^^tfX^lc^W t & £ S IB ft *§ </> 

[0ffi<7>fS^ 5 5ri&HH] 
[ 0 0 5 2 ] 

[01] x. ylHl^)2iX7C^n V ^ 

[02] 2iX7C*nv>fKftJj»*^-*ei. 

[0 3] 3 iX7C ^ n y >f H ft*ft<7> a . ^£#$^t0„ 

[0 4] ttilttfcft$i:<7)i1l$^tr7 7„ 

[0 5] If^t^tlSS. 

[0 6] 3)X7C?n'M Hffifflfti^fJ (ft*§UcSft&*£&. J^Fffft&ft*¥g£>rt 

[0 7 ] 3 YKjz ?nv^F ffireft«fc^$6^'b<7>ft*SJ* t ft*<o L 7 

[0 8] wai.mmft'&^^-txnm. 
[09] * 7- v >r > ft*&*^-tifc«ei. 



[mm%] mm 
im 1 ] 




z 




[0 4 ] 




0.5 1 



[0 5 ] 



STEPl 



3 ^7C^ n V 



STEP2 



STEP3 

• Kill 



[0 6 ] 



im ? ] 



1.6 
\A 
1.2 
1 

0.8 
0.6 
0.4 
0.2 
0 




!' cun/esl 

*CtfrVC2 

■curvo3 



[0 8 ] 



i fttpaTtttt h (S2) 

I " 



B#^J ~(S3) 



- (S4) 



- (S5) 



(S6) 



P = P(s) , E = E(s) 



s = s (t) 



t = n "At 



P = P(t) , E = E(t) 



8 i =8 i (Pj , Ej ) 
0 i =0 i (t) 



[m 9 ] 




mm 

[fim^S] ^^B^^Kfi^ijp^^ti. 3 )X tu Myrtle ft .WJcJf^ «bttfc;&#JWfc* m 
S3iX7C^nv>fHftiji(3iX7C^av>fH-fer>>h) ^ffluTMU 3^3^7U^ 
[38JREI] 133 
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